#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
#include <iostream>
#include "geometry_msgs/Twist.h"
#include <signal.h>
#include "TCPClient.h"
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;
TCPClient tcp;
#define M812X_CHN_NUMBER	6
MatrixXd m_dResultChValue=MatrixXd::Zero(1,M812X_CHN_NUMBER); //engineering output of each channel

void sig_exit(int s)
{
  tcp.exit();
  exit(0);
}

bool ConfigSystem(void)
{
  // Make sure that the parameter below is "(A01,A02,A03,A04,A05,A06);E;1;(WMA:1)\r\n", 
  // where a 'E' is included, not 'C'.

  tcp.Send("AT+SGDM=?\r\n");
  string rec = tcp.read();
  ROS_INFO("##############");
  ROS_INFO("Important! Make sure that the response [rec_SGDM] below is: '(A01,A02,A03,A04,A05,A06);E;1;(WMA:1)'");
  ROS_INFO("1. SGDM should include 'E' before WMA (Engineering output), not 'C' (ADC output).");
  ROS_INFO("2. WMA means filter parameters, 1 indicate raw newest data without filtering.");
  if( rec != "" ) {
    ROS_WARN("[rec_SGDM] Response: %s", rec.c_str()); 
  }
  else {
    std::cout << "Server Not Response: Exit Now!"  << endl;
    return false;
  }
  // ### If you want to solve the abot problem, uncomment the following lines to set once ### 
  std::cout << "Set Sensor channels ..." << endl;
  tcp.Send("AT+SGDM=(A01,A02,A03,A04,A05,A06);E;1;(WMA:1)\r\n");
  std::cout << tcp.read();

  tcp.Send("AT+SMPF=?\r\n");
  string rec_hz = tcp.read();
  if( rec != "" ) 
  {
    ROS_INFO("##############");
    ROS_INFO("Make Sure the Publish Rate and Force Rate is set as the same.");
    ROS_WARN("Force Sensing Rate is set to: %s", rec_hz.c_str());
  }
  else {
    std::cout << "Server Not Response: Exit Now!" << endl;
    return false;
  }
  // ### If you want to solve the abot problem, uncomment the following lines to set once ### 
  std::cout << "Set Sensor Sensing Rate ..." << endl;
  tcp.Send("AT+SMPF=500\r\n");
  std::cout << tcp.read();

  return true;
}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "sri_forcesensor");
  ros::NodeHandle n;

  if (argc != 3) {
      std::cerr << "Usage: rosrun rosnode <IP address>  <Publish Rate>" << std::endl;
      std::cerr << "Default IP is: 192.168.0.108 (Network of PC can be set to 192.168.0.2)" << std::endl;
      std::cerr << "Default Publish Rate can be 200" << std::endl;
      return 1;
  }

  std::string ipAddress = argv[1];
  float l_rate = std::stof(argv[2]);
  std::cout << "IP Address: " << ipAddress << std::endl << "Publish rate:" << l_rate << std::endl;

  signal(SIGINT, sig_exit);
  if(tcp.setup(ipAddress, 4008)==true)
  {
    ROS_INFO("Force sensor has been connected! \n ");
  }else{
    ROS_ERROR("Force sensor connection failed!");
    return -1;
  }
  // initialize the setting of the force sensor
  if (ConfigSystem() == false) return -1;

  // get real time force sensor data
  ROS_INFO("Start to Publish Rate: %f Hz", l_rate);
  tcp.Send("AT+GSD\r\n");

  geometry_msgs::Twist Forcevalue;
  ros::Publisher chatter_pub = n.advertise<geometry_msgs::Twist>("sri_force", 1000);

  ros::Rate loop_rate(l_rate);
  while (ros::ok())
  {
    // geometry_msgs::Twist 
    tcp.readrecieveBuffer_IEEEfloat32(m_dResultChValue);

    Forcevalue.linear.x=m_dResultChValue(0,0);
    Forcevalue.linear.y=m_dResultChValue(0,1);
    Forcevalue.linear.z=m_dResultChValue(0,2);
    Forcevalue.angular.x=m_dResultChValue(0,3);
    Forcevalue.angular.y=m_dResultChValue(0,4);
    Forcevalue.angular.z=m_dResultChValue(0,5);

    chatter_pub.publish(Forcevalue);
    // std::cout << m_dResultChValue << std::endl;

    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}

